#pragma config(Sensor, in1,    right,               sensorReflection)
#pragma config(Sensor, in2,    middle,              sensorReflection)
#pragma config(Sensor, in3,    left,                sensorReflection)
#pragma config(Sensor, in4,    back,                sensorReflection)
#pragma config(Sensor, in6,    button,              sensorTouch)
#pragma config(Motor,  port2,           motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  port4,           motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  port5,           motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void radioPrep();
void radioControl();
void checkLights();
void amIOnTheLine();
void goBabyGo();
void turnLeft();
void turnRight();

int radio = 0;
int lightLeft = 0;
int lightMiddle = 0;
int lightRight = 0;
int lightBack = 0;
int turnSpeed = 45;
int speed = 85;

task main()
{
  radioPrep();
  while(true)
  {
    radio = 0;
	  while(radio == 0)
	  {
	    radioControl();
	    radio = vexRT(Ch5);
	  }
	  radio = 0;
	  wait1Msec(500);
	  while(radio == 0)
	  {
	    radio = vexRT(Ch5);
	    goBabyGo();
	    checkLights();
	    amIOnTheLine();
	  }
	  wait1Msec(500);
  }
}

void radioPrep()
{
  bMotorFlippedMode[port3] = 1;
  bMotorFlippedMode[port5] = 1;
  bVexAutonomousMode = false;
}

void radioControl()
{
    motor[port2] = vexRT(Ch2);
    motor[port3] = vexRT(Ch3);
    motor[port4] = vexRT(Ch2);
    motor[port5] = vexRT(Ch3);
}

void checkLights()
{
   lightLeft = SensorValue(left);
   lightMiddle = SensorValue(middle);
   lightRight = SensorValue(right);
   lightBack = SensorValue(back);
}
void amIOnTheLine()
{
  while((lightMiddle > (lightBack + 30)) && radio == 0)
  {
    radio = vexRT(Ch5);
    checkLights();
    turnLeft();
    wait1Msec(700);
  }

}

void goBabyGo()
{
  motor[port2]= speed;
  motor[port3]= speed;
  motor[port4]= speed;
  motor[port5]= speed;
}
void turnLeft()
{
  motor[port2] = -turnSpeed;
  motor[port3] = turnSpeed;
  motor[port4] = -turnSpeed;
  motor[port5] = turnSpeed;
}
void turnRight()
{
  motor[port2] = turnSpeed;
  motor[port3] = -turnSpeed;
  motor[port4] = turnSpeed;
  motor[port5] = -turnSpeed;
}
